#!/usr/bin/env python3
# encoding: utf-8

import rospy
from std_msgs.msg import String
import curses

class VehicleCommandPublisher:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('vehicle_command_publisher', anonymous=True)

        # 创建一个Publisher，发布到/vehicle_command话题
        self.pub = rospy.Publisher('/vehicle_command', String, queue_size=10)

        # 设置发布频率
        self.rate = rospy.Rate(10)  # 10 Hz

    def publish_command(self, command):
        # 发布消息
        rospy.loginfo("Publishing: {}".format(command))
        self.pub.publish(command)

    def run(self, stdscr):
        # 清屏
        stdscr.clear()
        stdscr.addstr("Press 'w' to move forward, 's' to stop. Press 'q' to quit.\n")
        stdscr.refresh()

        while not rospy.is_shutdown():
            key = stdscr.getch()
            if key == ord('w'):
                self.publish_command("move_forward")
            elif key == ord('s'):
                self.publish_command("move_backward")
            elif key == ord('a'):
                self.publish_command("turn_left")
            elif key == ord('d'):
                self.publish_command("turn_right")
            elif key == ord('e'):
                self.publish_command("stop")
            elif key == ord('q'):
                break  # 退出循环

            self.rate.sleep()  # 按照设定的频率休眠

if __name__ == '__main__':
    try:
        vehicle_command_publisher = VehicleCommandPublisher()
        curses.wrapper(vehicle_command_publisher.run)
    except rospy.ROSInterruptException:
        pass